function [PathPoints, st, ErrCode, info] = RPLNSNPath(vehx, vehy, vehth, tpmx, tpmy, tpmth, R1, R2, PSInfo, VEHInfo, ParamInfo)
% R+L-S-模式
% 右转前进 + 左转后退 + 直行后退
% 输入数据处理
Cr2SafeDist = ParamInfo.Cr2SafeDist;
ymin = ParamInfo.ymin;
cr2x = PSInfo.cr2x;
cr2y = PSInfo.cr2y;
VehWidth = VEHInfo.VehWidth;
VehRear  = VEHInfo.VehRear;
VehLength = VEHInfo.VehLength;

% 初始化返回结果 
info = struct('Fcount',uint8(0),'Ncount',uint8(0),'count',uint8(0),'c2dist',single(0),'ymin',single(0));
info.c2dist = Cr2SafeDist;
info.count = uint8(0);
ErrCode = uint8(0);
st = true;
PathPoint  = struct('XPos',single(0),'YPos',single(0),'Headangle',single(0),'Kappa',single(0),'PathLength',single(0),'PathType',Pt_Typ_PathType_en.Initial);
PathPoints = repmat(PathPoint,[1 8]);

% 关键点计算
% 1 求NP0圆心1
C0x = vehx + R1*cos(deg2rad(vehth) - single(pi/2));
C0y = vehy + R1*sin(deg2rad(vehth) - single(pi/2));
% 2 求圆心1到tpm线的距离
a2 =  sin(deg2rad(tpmth));
b2 = -cos(deg2rad(tpmth));
c2 = tpmy*cos(deg2rad(tpmth)) - tpmx*sin(deg2rad(tpmth));
d1 = abs(a2*C0x + b2*C0y + c2)/sqrt(a2^2 + b2^2);

% 第一段圆弧与tpm相离
if d1 > R1
    st = false;
    ErrCode = uint8(14);
    return;
end  
% 3 圆心1到切点2法线的距离
%此处无需校验平方差正负
d2 = sqrt((R1+R2)^2 - (d1+R2)^2);
% 4 求圆心2的坐标
C1x = C0x - d2*cos(deg2rad(tpmth)) + (d1+R2)*cos(deg2rad(tpmth) + single(pi/2));
C1y = C0y - d2*sin(deg2rad(tpmth)) + (d1+R2)*sin(deg2rad(tpmth) + single(pi/2));
% 5 求圆弧1与圆弧2的切点1坐标
th1 = atan2(C1y-C0y, C1x-C0x);
dth1 = abs(OffCPIP_angdiff(th1, deg2rad(vehth)+single(pi/2)));
T1x  = C0x + R1*cos(th1);
T1y  = C0y + R1*sin(th1);
T1th = vehth - rad2deg(dth1); 

% 6 判断NP1的右前点是否小于ymin
NP1RFy = T1y + (VehLength - VehRear)*sin(deg2rad(T1th)) + VehWidth/single(2)*sin(deg2rad(T1th)-single(pi/2));
% NP1右前点超出ymin
if NP1RFy < ymin
    info.ymin = NP1RFy;
    st = false;
    ErrCode = uint8(15);
    return;
end

% 7 求tpm与圆弧2的切点2
T2x  = C1x + R2*cos(deg2rad(tpmth)-single(pi/2));
T2y  = C1y + R2*sin(deg2rad(tpmth)-single(pi/2));
T2th = tpmth;
dth2 = abs(OffCPIP_angdiff(deg2rad(T1th), deg2rad(tpmth)));

% 判断切点是否在tpm点在后方
dotval = (T2x - tpmx)*cos(deg2rad(tpmth)) + (T2y - tpmy)*sin(deg2rad(tpmth));
% 第三段直线段不存在
if dotval < single(0)   
    st = false;
    ErrCode = uint8(16);
    return;
end

% 判断2点到圆弧段的最短距离是否满足碰撞要求
dis = R2 - sqrt((C1x - cr2x)^2 + (C1y - cr2y)^2) - VehWidth/single(2);
% 圆弧段不满足碰撞要求
if dis < Cr2SafeDist && T2y > cr2y
    info.c2dist = dis;
    st = false;
    ErrCode = uint8(17);
    return;
end
% 判断第一段曲线是否过短
sl = R1*dth1;
if sl < single(0.5)
    st = false;
    ErrCode = uint8(5);
    return;
end
% 更新返回数据
PathPoints(1).XPos = vehx;
PathPoints(1).YPos = vehy;
PathPoints(1).Headangle = vehth;
PathPoints(1).PathType = Pt_Typ_PathType_en.FRCircle;
PathPoints(1).Kappa    = -single(1/R1);
PathPoints(1).PathLength = R1*dth1;

PathPoints(2).XPos = T1x;
PathPoints(2).YPos = T1y;
PathPoints(2).Headangle = T1th;
PathPoints(2).PathType = Pt_Typ_PathType_en.BLCircle;
PathPoints(2).Kappa    = single(1/R2);
PathPoints(2).PathLength = dth2*R2;

PathPoints(3).XPos = T2x;
PathPoints(3).YPos = T2y;
PathPoints(3).Headangle = T2th;
PathPoints(3).PathType = Pt_Typ_PathType_en.BLine;
PathPoints(3).Kappa    = single(0);
PathPoints(3).PathLength = sqrt((T2x-tpmx)^2+(T2y-tpmy)^2);

PathPoints(4).XPos = tpmx;
PathPoints(4).YPos = tpmy;
PathPoints(4).Headangle = tpmth;
PathPoints(4).PathType = Pt_Typ_PathType_en.Initial;
PathPoints(4).Kappa    = single(0);
PathPoints(4).PathLength = single(0);
info.count = uint8(3);
p1ok = OffCPIP_CheckKeyPathPoint(PathPoints(1),PathPoints(2));
p2ok = OffCPIP_CheckKeyPathPoint(PathPoints(2),PathPoints(3));
p3ok = OffCPIP_CheckKeyPathPoint(PathPoints(3),PathPoints(4));
if ~(p1ok&&p2ok&&p3ok)
    st = false;
end
end

    


